#include <chrono>
#include <memory>
#include <mutex>
#include <string>
#include <vector>

#include "rclcpp/rclcpp.hpp"
#include "rclcpp_components/register_node_macro.hpp"
#include "std_msgs/msg/string.hpp"

using namespace std::chrono_literals;

namespace multi_launch_demo
{

class ComponentAggregator : public rclcpp::Node
{
   public:
    ComponentAggregator(const rclcpp::NodeOptions &options) : Node("component_aggregator", options)
    {
        // 声明参数
        this->declare_parameter("aggregation_window_ms", 2000);
        this->declare_parameter("max_messages_per_batch", 10);

        // 获取参数
        aggregation_window_ms_ = this->get_parameter("aggregation_window_ms").as_int();
        max_messages_per_batch_ = this->get_parameter("max_messages_per_batch").as_int();

        // 创建订阅者 - 接收处理后的数据
        processed_subscription_ = this->create_subscription<std_msgs::msg::String>(
            "processed_data", 10, [this](const std_msgs::msg::String::SharedPtr msg) { this->aggregate_message(msg); });

        // 创建发布者 - 发布聚合后的数据
        aggregated_publisher_ = this->create_publisher<std_msgs::msg::String>("aggregated_data", 10);

        // 创建定时器 - 定期发布聚合结果
        aggregation_timer_ = this->create_wall_timer(std::chrono::milliseconds(aggregation_window_ms_),
                                                     [this]() -> void { this->publish_aggregated_data(); });

        RCLCPP_INFO(this->get_logger(), "Component Aggregator started - Window: %dms, Max batch: %d",
                    aggregation_window_ms_, max_messages_per_batch_);
    }

   private:
    void aggregate_message(const std_msgs::msg::String::SharedPtr msg)
    {
        std::lock_guard<std::mutex> lock(messages_mutex_);

        // 添加到消息队列
        pending_messages_.push_back(msg->data);

        // 如果达到最大批次大小，立即发布
        if (static_cast<int>(pending_messages_.size()) >= max_messages_per_batch_)
        {
            publish_aggregated_data();
        }

        RCLCPP_DEBUG(this->get_logger(), "Aggregated message %d: '%s'", static_cast<int>(pending_messages_.size()),
                     msg->data.c_str());
    }

    void publish_aggregated_data()
    {
        std::lock_guard<std::mutex> lock(messages_mutex_);

        if (pending_messages_.empty())
        {
            return;
        }

        // 创建聚合消息
        auto aggregated_msg = std_msgs::msg::String();
        aggregated_msg.data = "[AGGREGATED] Batch " + std::to_string(batch_count_++) + ": ";

        // 聚合所有待处理的消息
        for (size_t i = 0; i < pending_messages_.size(); ++i)
        {
            if (i > 0)
            {
                aggregated_msg.data += " | ";
            }
            aggregated_msg.data += pending_messages_[i];
        }

        // 添加统计信息
        aggregated_msg.data += " [Total: " + std::to_string(pending_messages_.size()) + " messages]";

        // 发布聚合消息
        aggregated_publisher_->publish(aggregated_msg);

        RCLCPP_INFO(this->get_logger(), "Published aggregated batch %d with %d messages", batch_count_ - 1,
                    static_cast<int>(pending_messages_.size()));

        // 清空消息队列
        pending_messages_.clear();
    }

    rclcpp::Subscription<std_msgs::msg::String>::SharedPtr processed_subscription_;
    rclcpp::Publisher<std_msgs::msg::String>::SharedPtr aggregated_publisher_;
    rclcpp::TimerBase::SharedPtr aggregation_timer_;

    std::vector<std::string> pending_messages_;
    std::mutex messages_mutex_;

    int aggregation_window_ms_;
    int max_messages_per_batch_;
    int batch_count_ = 0;
};

}  // namespace multi_launch_demo

RCLCPP_COMPONENTS_REGISTER_NODE(multi_launch_demo::ComponentAggregator)
